#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>

class SerialNode {
public:
    SerialNode(const std::string& port, uint32_t baudrate) {
        // 初始化串口
        try {
            ser.setPort(port);
            ser.setBaudrate(baudrate);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000);
            ser.setTimeout(to);
            ser.open();
        } catch (serial::IOException& e) {
            ROS_ERROR_STREAM("Unable to open port");
        }

        if (ser.isOpen()) {
            ROS_INFO_STREAM("Serial Port initialized");
        } else {
            ROS_ERROR_STREAM("Serial Port not initialized");
        }

        // 定义发布者和订阅者
        pub = nh.advertise<std_msgs::String>("/serial_read", 1000);
        sub = nh.subscribe("/serial_write", 1000, &SerialNode::writeSerial, this);
    }

    void writeSerial(const std_msgs::String::ConstPtr& msg) {
        // 当收到订阅消息时写入串口
        if (ser.isOpen()) {
            ser.write(msg->data);
            ROS_INFO_STREAM("Sent to serial: " << msg->data);
        } else {
            ROS_ERROR_STREAM("Serial port not open");
        }
    }

    void readSerial() {
        // 从串口读取数据并发布
        if (ser.isOpen()) {
            std_msgs::String result;
            result.data = ser.readline(65536, "\n");
            if (!result.data.empty()) {
                pub.publish(result);
                ROS_INFO_STREAM("Received from serial: " << result.data);
            }
        } else {
            ROS_ERROR_STREAM("Serial port not open");
        }
    }

    void spin() {
        ros::Rate loop_rate(10); // 10 Hz
        while (ros::ok()) {
            readSerial();
            ros::spinOnce();
            loop_rate.sleep();
        }
    }

private:
    ros::NodeHandle nh;
    ros::Publisher pub;
    ros::Subscriber sub;
    serial::Serial ser;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "serial_node");

    ros::NodeHandle private_nh("~");
    std::string port;
    int baudrate;
    ROS_INFO_STREAM("main start initialize---");
    private_nh.param("port", port, std::string("/dev/ttyUSB0"));
    private_nh.param("baudrate", baudrate, 115200);

    SerialNode node(port, baudrate);
    node.spin();

    return 0;
}
